#include "bitmap.h"
#include "print.h"

int8_t bitmap_init(struct bitmap* p, uint8_t* addr, uint32_t len)
{
	int i;
	
	if (NULL == p || NULL == addr || len == 0)
		return -1;
	
	p->addr = addr;
	p->len = len;
	
	for (i = 0; i < p->len; ++i)
	{
		p->addr[i] = 0;
	}
	
	return 0;
}

int32_t bitmap_scan_test(struct bitmap* p, uint32_t count)
{
	int32_t index = -1, curr_count = 0, i, j;
	
	if (NULL == p || NULL == p->addr || count == 0)
		return -1;
	
	for (i = 0; i < p->len && curr_count < count; ++i)
	{
		for (j = 0; j < 8 && curr_count < count; ++j)
		{
			if ((p->addr[i] & (1 << j)) == 0)
			{
				p->addr[i] |= (1 << j);
				++curr_count;
				if (curr_count == 1)
					index = i * 8 + j;
			}
			else
			{
				index = -1;
				curr_count = 0;
			}
		}
	}
	
	return index;
}

void bitmap_set(struct bitmap* p, int32_t index)
{
	int32_t byte, bit;
	
	if (NULL == p || NULL == p->addr || index < 0)
		return;
	
	byte = index / 8;
	bit = index % 8;
	
	if (byte >= p->len)
		return;
	
	p->addr[byte] |= 1 << bit;
}

void bitmap_clear(struct bitmap* p, int32_t index)
{
	int32_t byte, bit;
	
	if (NULL == p || NULL == p->addr || index < 0)
		return;
	
	byte = index / 8;
	bit = index % 8;
	
	if (byte >= p->len)
		return;
	
	p->addr[byte] &= ~(1 << bit);
}

int8_t bitmap_get(struct bitmap* p, int32_t index)
{
	int32_t byte, bit;
	
	if (NULL == p || NULL == p->addr || index < 0)
		return -1;
	
	byte = index / 8;
	bit = index % 8;
	
	if (byte >= p->len)
		return -1;
	
	return (p->addr[byte] & (1 << bit)) >> 1;
}
